/*****************************************************************************
 * $Workfile:: joystick.c                                                    $
 * $Revision:: 1                                                             $
 * $JustDate::  1/27/12                                                      $
 *
 *      GENIE V ECU
 *      Platform Controller interface
 *****************************************************************************/

#include "common.h"
#include "serial.h"
#include "joystick.h"
#include "nvm.h"
#include "faults.h"
#include "main.h"

#define JOYSTICK_OFFSET_COMPENSATION lift_setup.bJoystickOffsetCompensation
#define ROCKER_OFFSET_COMPENSATION   lift_setup.bJoystickOffsetCompensation
#define DAMPENING_PERCENT            lift_setup.bNeutralSteeringPwm

struct joystick JOYSTICK_Data;
//struct rocker   ROCKER_Data;
struct platform PLATFORM_Data;

/*******************************************************************************
 *
 * PROCEDURE:  main()
 *
 * DESCRIPTION: It write initial values to PLATFORM_Data structure.
 *
 * VARIABLE NAME  R/W DESCRIPTION
 * ---------------- --- --------------------------------------------------------
 * PLATFORM_Data    R/W clear
 ******************************************************************************/

void
JoystickInit(void)
{
   UINT8 ix;
   UINT8 * pb;

   pb = (UINT8 *)&PLATFORM_Data;

   for(ix = 0; ix < sizeof(struct platform); ix++)
   {
      *pb++ = 0;
   }
}

/*******************************************************************************
 *
 * PROCEDURE:  JoystickAdjust()
 *
 * DESCRIPTION: It detects joystick neutral zone offset  and adjust counts from
 *              the offset.
 *
 * VARIABLE NAME  R/W DESCRIPTION
 * ---------------- --- --------------------------------------------------------
 * JOYSTICK_Data_Data    R/W clear
 ******************************************************************************/

void JoystickAdjust(void)
{
   //    static INT32 lJoystickAve = 0l;
   INT16 nIn;
   INT16 nOut;
   INT16 nJoystick;

   nJoystick = JOYSTICK_RAW_COUNTS;

   switch(JOYSTICK_Data.eState)
   {
      /*
       * this state clears joystick counts and disable system by
       * overwriting ENABLE switch.
       */
      default:
      case JOYSTICK_FAILURE:

         JOYSTICK_Data.fEnableSwitchFault = 1;
         JOYSTICK_Data.eState = JOYSTICK_FAILURE;
         PLATFORM_Data.fEnable = 0;
         nJoystick = 0;

         break;

      /*
       * this state detects joystick offset and
       * check for faulty platform switches.
       * The state is executed once after power up i.e. at platform
       * initialization.
       */
      case JOYSTICK_OFFSET:

         /*
          * It checks for left and right buttons failure.
          * The platform will be fully functional.
          */
         if(PLATFORM_Data.fLeft && !JOYSTICK_Data.fLeftSwitchFault)
         {
            JOYSTICK_Data.fLeftSwitchFault = 1;
            NVM_SaveFault(FAULT_JOYSTICK_LEFT_SWITCH);
            // set fault word 1 bit 7, by Shawn @20160812
            FaultWord1 |= 0x0080;
            // end Shawn@20160812
			unCanFault1.word |= 0x0080;
         }
         if(PLATFORM_Data.fRight && !JOYSTICK_Data.fRightSwitchFault)
         {
            JOYSTICK_Data.fRightSwitchFault = 1;
            NVM_SaveFault(FAULT_JOYSTICK_RIGHT_SWITCH);
            // set fault word 1 bit 8, by Shawn @20160812
            FaultWord1 |= 0x0100;
            // end Shawn@20160812
			unCanFault1.word |= 0x0100;
         }

         /*
          * It checks for intrlock switch failure.
          * The platform is disabled if the failure occur.
          */
         if(PLATFORM_Data.fEnable)
         {
            NVM_SaveFault(FAULT_JOYSTICK_ENABLE_SWITCH);
            // set fault word 1 bit 9, by Shawn @20160812
            FaultWord1 |= 0x0200;
            // end Shawn@20160812
            JOYSTICK_Data.fEnableSwitchFault = 1;
            JOYSTICK_Data.eState = JOYSTICK_FAILURE;
            JOYSTICK_Data.fPlatformReady = 1;
			unCanFault1.word |= 0x0200;
         }
         //
         // accumulate neutral average
         //
         else if(JOYSTICK_Data.bCount < 4)
         {
            JOYSTICK_Data.nCenter += nJoystick;
            JOYSTICK_Data.bCount++;
         }
         //
         // check initial offset and record left/right/offset faults.
         //
         else if(MAIN_WAIT_PLATFORM_INIT == MAIN_Data.eState ||
                 MAIN_TRIP_ANGLE_SAVE == MAIN_Data.eState ||
                 MAIN_BRAKE_RELEASE == MAIN_Data.eState ||
                 MAIN_SPEED_ADJUST == MAIN_Data.eState)
		 {

            JOYSTICK_Data.fPlatformReady = 1;

            nIn = JOYSTICK_Data.nCenter / JOYSTICK_Data.bCount;
            JOYSTICK_Data.nCenter = nIn;

            if(nIn < 0)
            {
               nIn = 0 - nIn;
            }

            /*
             * if offset is too big don't adjust the
             * counts. The control loop
             * will allow only slowest drive mode.
             * NOTE! The out of netral fault is NOT detected
             * if parameters are not initialized.
             */
            if(nIn > JOYSTICK_OFFSET_COMPENSATION)
            {
               JOYSTICK_Data.fNeutralFault = 1;
               NVM_SaveFault(FAULT_JOYSTICK_OUT_OF_NEUTRAL);
               // set fault word 1 bit 10, by Shawn @20160812
               FaultWord1 |= 0x0400;
               // end Shawn@20160812
               JOYSTICK_Data.eState = JOYSTICK_NEUTRAL_FAILURE;
			   unCanFault1.word |= 0x0400;
            }
            else
            {
               JOYSTICK_Data.eState = JOYSTICK_ADJUST;
            }

         }
         //
         // system initialization failure
         // should never be in this state
         //
         else
         {
            /*
             * mimic enable switch failure
             */
            JOYSTICK_Data.fEnableSwitchFault = 1;
			if(MAIN_Data.eState != MAIN_GPS_UNLOCK_TEMPORARY_RANDOM_CODE && MAIN_Data.eState != MAIN_GPS_UNLOCK_TEMPORARY_CHECK_CODE)
            	NVM_SaveFault(FAULT_JOYSTICK_ENABLE_SWITCH);
            	// set fault word 1 bit 9, by Shawn @20160812
            FaultWord1 |= 0x0200;
            JOYSTICK_Data.eState = JOYSTICK_FAILURE;
            JOYSTICK_Data.fPlatformReady = 1;
            unCanFault1.word |= 0x0200;
         }
         /*
          * platfrom is not ready yet. make joystick inactive.
          */
         nJoystick = 0;
         PLATFORM_Data.fEnable = 0;
         break;

      /*
       * The initial hoystick offset is too big.
       * This state doesn't adjust row counts.
       */
      case JOYSTICK_NEUTRAL_FAILURE:
         // return row counts
         break;

      /*
       * This state redestribute the row counts from new neutral zone
       * defined by joystick initial offset.
       */
      case JOYSTICK_ADJUST:
         if(SW_ENABLE_RAW)
            ;
         else
            nJoystick = JOYSTICK_Data.nCenter;
         nIn = nJoystick;

         // scale only short branch, and shift long branch
         if(nIn > JOYSTICK_Data.nCenter && JOYSTICK_Data.nCenter > 0)
         {  //short branch
            nOut = X_MAX - JOYSTICK_Data.nCenter;
            nOut =
               ((INT16)X_MAX * (INT16)(nIn - JOYSTICK_Data.nCenter) + (nOut >> 1))
               / nOut;
         }
         else if(nIn < JOYSTICK_Data.nCenter && JOYSTICK_Data.nCenter < 0)
         {  //short branch
            nOut = X_MAX + JOYSTICK_Data.nCenter;
            nOut =
               ((INT16)X_MAX * (INT16)(nIn - JOYSTICK_Data.nCenter) - (nOut >> 1))
               / nOut;
         }
         else
         {  // long branthes
            nOut = nIn - JOYSTICK_Data.nCenter;
         }

         nJoystick = nOut;
         break;
   }

   // saturate value
   if(nJoystick > X_MAX)
   {
      nJoystick = X_MAX;
   }
   else if(nJoystick < (0 - X_MAX))
   {
      nJoystick = 0 - X_MAX;
   }

   // shift out of neutral zone
   if(nJoystick >= NEUTRAL_ZONE)
   {
      nJoystick -= NEUTRAL_ZONE;
   }
   else if(nJoystick <= (0 - NEUTRAL_ZONE))
   {
      nJoystick += NEUTRAL_ZONE;
   }
   else
   {
      nJoystick = 0;
   }

   Y_AXIS_COUNTS = nJoystick;
}





